using System;
using System.Collections;
using System.Collections.Generic;
using System.Runtime.CompilerServices;
using System.Threading;
using LightCAD.MathLib;

namespace LightCAD.MathLib
{
    public sealed class Matrix3 : IIOArray
    {
        #region scope properties or methods
        //private static Matrix3 _m3 = new Matrix3();
        [MethodImpl((MethodImplOptions)768)]
        private static Matrix3Context GetContext()
        {
            return ThreadContext.GetCurrThreadContext().Matrix3Ctx; 
        }
        #endregion

        #region Properties

        public double[] Elements;

        #endregion
        public static int newCount = 0;
        #region constructor
        public Matrix3()
        {
            Interlocked.Add(ref newCount, 1);
            Elements = new double[] {
                    1, 0, 0,
                    0, 1, 0,
                    0, 0, 1
                };
        }
        public Matrix3(double n11, double n12, double n13, double n21, double n22, double n23, double n31, double n32, double n33)
            :this()
        {
            this.Set(n11,n12, n13, n21, n22, n23, n31, n32, n33);
        }
        #endregion

        #region methods
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Set(double n11, double n12, double n13, double n21, double n22, double n23, double n31, double n32, double n33)
        {
            double[] te = Elements;
            te[0] = n11; te[1] = n21; te[2] = n31;
            te[3] = n12; te[4] = n22; te[5] = n32;
            te[6] = n13; te[7] = n23; te[8] = n33;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool IsIdentity()
        {
            double[] te = Elements;
            return te[0] == 1 && te[1] == 0 && te[2] == 0 &&
                   te[3] == 0 && te[4] == 1 && te[5] == 0 &&
                   te[6] == 0 && te[7] == 0 && te[8] == 1;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 SetIdentity()
        {
            Set(
                1, 0, 0,
                0, 1, 0,
                0, 0, 1
            );
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Copy(Matrix3 m)
        {
            double[] te = Elements;
            double[] me = m.Elements;
            te[0] = me[0]; te[1] = me[1]; te[2] = me[2];
            te[3] = me[3]; te[4] = me[4]; te[5] = me[5];
            te[6] = me[6]; te[7] = me[7]; te[8] = me[8];
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 ExtractBasis(Vector3 xAxis, Vector3 yAxis, Vector3 zAxis)
        {
            xAxis.SetFromMatrix3Column(this, 0);
            yAxis.SetFromMatrix3Column(this, 1);
            zAxis.SetFromMatrix3Column(this, 2);
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 SetFromMatrix4(Matrix4 m)
        {
            double[] me = m.Elements;
            Set(
                me[0], me[4], me[8],
                me[1], me[5], me[9],
                me[2], me[6], me[10]
            );
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Multiply(Matrix3 m)
        {
            return MultiplyMatrices(this, m);
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Premultiply(Matrix3 m)
        {
            return MultiplyMatrices(m, this);
        }
        public Matrix3 MultiplyMatrices(Matrix3 a, Matrix3 b)
        {
            double[] ae = a.Elements;
            double[] be = b.Elements;
            double[] te = Elements;
            double a11 = ae[0], a12 = ae[3], a13 = ae[6];
            double a21 = ae[1], a22 = ae[4], a23 = ae[7];
            double a31 = ae[2], a32 = ae[5], a33 = ae[8];
            double b11 = be[0], b12 = be[3], b13 = be[6];
            double b21 = be[1], b22 = be[4], b23 = be[7];
            double b31 = be[2], b32 = be[5], b33 = be[8];
            te[0] = a11 * b11 + a12 * b21 + a13 * b31;
            te[3] = a11 * b12 + a12 * b22 + a13 * b32;
            te[6] = a11 * b13 + a12 * b23 + a13 * b33;
            te[1] = a21 * b11 + a22 * b21 + a23 * b31;
            te[4] = a21 * b12 + a22 * b22 + a23 * b32;
            te[7] = a21 * b13 + a22 * b23 + a23 * b33;
            te[2] = a31 * b11 + a32 * b21 + a33 * b31;
            te[5] = a31 * b12 + a32 * b22 + a33 * b32;
            te[8] = a31 * b13 + a32 * b23 + a33 * b33;
            return this;
        }
        public Matrix3 MultiplyScalar(double s)
        {
            double[] te = Elements;
            te[0] *= s; te[3] *= s; te[6] *= s;
            te[1] *= s; te[4] *= s; te[7] *= s;
            te[2] *= s; te[5] *= s; te[8] *= s;
            return this;
        }
        public double Determinant()
        {
            double[] te = Elements;
            double a = te[0], b = te[1], c = te[2],
                d = te[3], e = te[4], f = te[5],
                g = te[6], h = te[7], i = te[8];
            return a * e * i - a * f * h - b * d * i + b * f * g + c * d * h - c * e * g;
        }
        public Matrix3 Invert()
        {
            double[] te = Elements;
            double n11 = te[0], n21 = te[1], n31 = te[2],
               n12 = te[3], n22 = te[4], n32 = te[5],
               n13 = te[6], n23 = te[7], n33 = te[8],
               t11 = n33 * n22 - n32 * n23,
               t12 = n32 * n13 - n33 * n12,
               t13 = n23 * n12 - n22 * n13,
               det = n11 * t11 + n21 * t12 + n31 * t13;
            if (det == 0) return Set(0, 0, 0, 0, 0, 0, 0, 0, 0);
            double detInv = 1 / det;
            te[0] = t11 * detInv;
            te[1] = (n31 * n23 - n33 * n21) * detInv;
            te[2] = (n32 * n21 - n31 * n22) * detInv;
            te[3] = t12 * detInv;
            te[4] = (n33 * n11 - n31 * n13) * detInv;
            te[5] = (n31 * n12 - n32 * n11) * detInv;
            te[6] = t13 * detInv;
            te[7] = (n21 * n13 - n23 * n11) * detInv;
            te[8] = (n22 * n11 - n21 * n12) * detInv;
            return this;
        }
        public Matrix3 Transpose()
        {
            double tmp;
            double[] m = Elements;
            tmp = m[1]; m[1] = m[3]; m[3] = tmp;
            tmp = m[2]; m[2] = m[6]; m[6] = tmp;
            tmp = m[5]; m[5] = m[7]; m[7] = tmp;
            return this;
        }
        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 GetNormalMatrix(Matrix4 matrix4)
        {
            return SetFromMatrix4(matrix4).Invert().Transpose();
        }
        public Matrix3 TransposeIntoArray(double[] r)
        {
            double[] m = Elements;
            r[0] = m[0];
            r[1] = m[3];
            r[2] = m[6];
            r[3] = m[1];
            r[4] = m[4];
            r[5] = m[7];
            r[6] = m[2];
            r[7] = m[5];
            r[8] = m[8];
            return this;
        }
        public Matrix3 SetUvTransform(double tx, double ty, double sx, double sy, double rotation, double cx, double cy)
        {
            double c = Math.Cos(rotation);
            double s = Math.Sin(rotation);
            Set(
                sx * c, sx * s, -sx * (c * cx + s * cy) + cx + tx,
                -sy * s, sy * c, -sy * (-s * cx + c * cy) + cy + ty,
                0, 0, 1
            );
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Scale(double sx, double sy)
        {
            var _m3 = GetContext()._m3;
            this.Premultiply(_m3.MakeScale(sx, sy));
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Rotate(double theta)
        {
            var _m3 = GetContext()._m3;
            this.Premultiply(_m3.MakeRotation(-theta));
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Translate(double tx, double ty)
        {
            var _m3 = GetContext()._m3;
            this.Premultiply(_m3.MakeTranslation(tx, ty));
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 MakeTranslation(double x, double y)
        {
            Set(
                1, 0, x,
                0, 1, y,
                0, 0, 1
            );
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 MakeRotation(double theta)
        {
            // counterclockwise
            double c = Math.Cos(theta);
            double s = Math.Sin(theta);
            Set(
                c, -s, 0,
                s, c, 0,
                0, 0, 1
            );
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 MakeScale(double x, double y)
        {
            Set(
                x, 0, 0,
                0, y, 0,
                0, 0, 1
            );
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(Matrix3 matrix)
        {
            double[] te = Elements;
            double[] me = matrix.Elements;
            for (int i = 0; i < 9; i++)
            {
                if (!MathEx.IsEqual(te[i], me[i])) return false;
            }
            return true;
        }
        [MethodImpl((MethodImplOptions)768)]
        public bool Equals(IIOArray obj) => Equals(obj as Matrix3);

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 FromArray(double[] array, int offset = 0)
        {
            for (int i = 0; i < 9; i++)
            {
                Elements[i] = array[i + offset];
            }
            return this;
        }

        [MethodImpl((MethodImplOptions)768)]
        IIOArray IIOArray.FromArray(double[] array, int offset)
        {
            return FromArray(array, offset);
        }
        public double[] ToArray(double[] array = null, int offset = 0)
        {
            if (array == null) array = new double[9];
            double[] te = Elements;
            array[offset] = te[0];
            array[offset + 1] = te[1];
            array[offset + 2] = te[2];
            array[offset + 3] = te[3];
            array[offset + 4] = te[4];
            array[offset + 5] = te[5];
            array[offset + 6] = te[6];
            array[offset + 7] = te[7];
            array[offset + 8] = te[8];
            return array;
        }

        [MethodImpl((MethodImplOptions)768)]
        public Matrix3 Clone()
        {
            return new Matrix3().FromArray(Elements);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector2 MultiplyPoint(Vector2 v)
        {
            var te = this.Elements;
            return new Vector2(
               te[0] * v.X + te[3] * v.Y + te[6],
               te[1] * v.X + te[4] * v.Y + te[7]);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector2 MultiplyPoint(double x, double y)
        {
            var te = this.Elements;
            return new Vector2(
               te[0] * x + te[3] * y + te[6],
              te[1] * x + te[4] * y + te[7]);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector2 MultiplyVector(Vector2 v)
        {
            var te = this.Elements;
            return new Vector2(
                te[0] * v.X + te[3] * v.Y,
               te[1] * v.X + te[4] * v.Y);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector2 MultiplyVector(double x, double y)
        {
            var te = this.Elements;
            return new Vector2(
               te[0] * x + te[3] * y,
               te[1] * x + te[4] * y);
        }


        [MethodImpl((MethodImplOptions)768)]
        public Vector2 GetTranslateVector()
        {
            var te = this.Elements;
            return new Vector2(te[6], te[7]);
        }

        [MethodImpl((MethodImplOptions)768)]
        public Vector2 GetScaleVector()
        {
            var te = this.Elements;
            //https://math.stackexchange.com/questions/13150/extracting-rotation-scale-values-from-2d-transformation-matrix/13165#13165
            return new Vector2(Math.Sign(te[0]) * Math.Sqrt(te[0] * te[0] + te[3] * te[3]), Math.Sign(te[1]) * Math.Sqrt(te[1] * te[1] + te[4] * te[4]));
        }

        [MethodImpl((MethodImplOptions)768)]
        public double GetAngle()
        {
            var te = this.Elements;
            return Math.Atan2(-te[3], te[0]);//Math.Atan2(m21, m22);
        }

        [MethodImpl((MethodImplOptions)768)]
        public override bool Equals(object obj)
        {
            if (!(obj is Matrix3))
                return false;

            return Equals((Matrix3)obj);
        }

        public static Matrix3 Identity => new Matrix3(
               ).SetIdentity();

        public static Matrix3 Zero => new Matrix3(
                    0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0,
                    0.0, 0.0, 0.0);




        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 RotateInRadian(double angle)
        {
            double cos = Math.Cos(angle);
            double sin = Math.Sin(angle);
            return new Matrix3(cos, -sin, 0.0, sin, cos, 0.0, 0.0, 0.0, 1.0);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 Rotate(double degreeAngle, Vector2 center)
        {
            return RotateInRadian(MathEx.DegreeToRadian(degreeAngle), center);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 RotateInRadian(double angle, Vector2 center)
        {
            // Arbitrary rotation.
            var c = Math.Cos(angle);
            var s = Math.Sin(angle);

            var x = center.X * (1 - c) + center.Y * s;
            var y = center.Y * (1 - c) - center.X * s;

            return new Matrix3(
                c, -s, x,
                s, c, y,
                0.0, 0.0, 1.0);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetCoord(Vector2 orgin, Vector2 xAxis, Vector2 yAxis)
        {
            return new Matrix3(
                xAxis.X, yAxis.X, orgin.X,
                xAxis.Y, yAxis.Y, orgin.Y,
                0.0, 0.0, 1.0);
        }



        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetScale(Vector2 v)
        {
            return new Matrix3(
                v.X, 0.0, 0.0,
                0.0, v.Y, 0.0,
                0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetScale(double scale)
        {
            return new Matrix3(
                scale, 0.0, 0.0,
                0.0, scale, 0.0,
                0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetScale(double scaleX, double scaleY)
        {
            return new Matrix3(
                scaleX, 0.0, 0.0,
                0.0, scaleY, 0.0,
                0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetScale(Vector2 scale, Vector2 center)
        {
            var z = center * (Vector2.One - scale);
            return new Matrix3(
                scale.X, 0.0, z.X,
                0.0, scale.Y, z.Y,
                0.0, 0.0, 1.0);
        }
        public static Matrix3 GetMirror(Vector2 axisStart, Vector2 axisEnd)
        {

            double A = (axisStart.Y - axisEnd.Y);
            double B = (axisEnd.X - axisStart.X);
            double C = (axisStart.X * axisEnd.Y - axisEnd.X * axisStart.Y);
            double D = 1 / (Math.Pow(A, 2) + Math.Pow(B, 2));
            return new Matrix3(
                D * (Math.Pow(B, 2) - Math.Pow(A, 2)), D * -2 * A * B, D * -2 * A * C,
               D * -2 * A * B, D * (Math.Pow(A, 2) - Math.Pow(B, 2)), D * -2 * B * C,
               0.0, 0.0, 1);
        }


        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetScale(double scale, Vector2 center)
        {
            var z = center * (Vector2.One - new Vector2(scale, scale));
            return new Matrix3(
                scale, 0.0, z.X,
                0.0, scale, z.Y,
                0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetTranslate(Vector2 v)
        {
            return new Matrix3(
                1.0, 0.0, v.X,
                0.0, 1.0, v.Y,
                0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetTranslate(double x, double y)
        {
            return new Matrix3(
                1.0, 0.0, x,
                0.0, 1.0, y,
                0.0, 0.0, 1.0);
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 GetMove(Vector2 startPoint, Vector2 endPoint)
        {
            return new Matrix3(
                1.0, 0.0, endPoint.X - startPoint.X,
                0.0, 1.0, endPoint.Y - startPoint.Y,
               0.0, 0.0, 1.0);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 operator *(Matrix3 a, Matrix3 b)
        {
            return Multiply(a, b);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 operator *(Matrix3 m, Vector2 v)
        {
            return Multiply(m, v);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static bool operator ==(Matrix3 a, Matrix3 b)
        {
            return Object.Equals(a, b);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static bool operator !=(Matrix3 a, Matrix3 b)
        {
            return !(a == b);
        }
        [MethodImpl((MethodImplOptions)768)]
        public static Matrix3 Multiply(Matrix3 a, Matrix3 b)
        {
            Matrix3 m = new Matrix3();
            m.MultiplyMatrices(a, b);
            return m;
        }

        [MethodImpl((MethodImplOptions)768)]
        public static Vector2 Multiply(Matrix3 m, Vector2 v)
        {
            var te = m.Elements;
            return new Vector2(
                te[0] * v.X + te[3] * v.Y,
               te[1] * v.X + te[4] * v.Y);
        }

        [MethodImpl((MethodImplOptions)768)]
        public double[] GetArray()
        {
            return this.Elements;
        }
        [MethodImpl((MethodImplOptions)768)]
        internal void SetArray(double[] array)
        {
            Array.Copy(array, this.Elements, 16);
        }

        #endregion

    }
    public sealed class Matrix3Context
    {
        public readonly Matrix3 _m3 = new Matrix3();
    }
}
